AL/CF-TR-1995-0120 


EFFICIENT  COORDINATION  OF 
AN  ANTHROPOMORPHIC  TELEMANIPULATION  SYSTEM 


Ming  Z.  Huang 

CREW  SYSTEMS  DIRECTORATE 
BIOACOUSTICS  AND  BIOCOMMUNICATIONS  DIVISION 
WRIGHT-PATTERSON  AFB  OH  45433-7901 


DECEMBER  1993 


19960910  090 


INTERIM  REPORT  FOR  THE  PERIOD  JUNE  1993  TO  AUGUST  1993 


Approved  for  public  release;  distribution  is  unlimited 

OTIC  QUAUTY  mSPBCTED  S 

AIR  FORCE  MATERIEL  COMMAND 
WRIGHT-PATTERSON  AIR  FORCE  BASE,  OHIO  45433-6573; 


NOTICE 


When  US  Gk)vemnient  drawings,  specifications,  or  other  data  are  used  for  any  purpose  other  than 
a  definitely  related  Government  procurement  operation,  the  Government  thereby  incurs  no 
responsibihty  nor  any  obligation  whatsoever,  and  the  fiict  that  the  Government  may  have 
formulated,  fiimished,  or  in  any  way  supplied  the  said  drawings,  specifications,  or  other  data,  is 
not  to  be  regarded  by  implication  or  otherwise,  as  in  any  manner,  licensing  the  holder  or  any  other 
person  or  corporation,  or  conveying  any  rights  or  permission  to  manufacture,  use  or  sell  any 
patented  invention  that  may  in  any  way  be  related  tho'eto. 

Please  do  not  request  copies  of  this  report  fi’om  the  Armstrong  Laboratory.  Additional  copies 
may  be  purchased  fi'om: 

National  Technical  Information  Service 
5285  Port  Royal  Road 
Springfield  VA  22161 

Federal  Government  agencies  and  their  contractors  registered  with  Defense  Technical 
Information  Center  should  direct  requests  for  copies  of  this  report  to: 

Defense  Technical  Information  Center 
Cameron  Station 
Alexandria  VA  223 14 

TECHNICAL  REVIEW  AND  APPROVAL 
AL/CF-TR-1995-0120 

This  report  has  been  reviewed  by  the  Office  of  Public  Affairs  (PA)  and  is  releasable  to  the 
National  Technical  Information  Serwce  (NTIS).  At  NTIS,  it  will  be  available  to  the  general 
public,  including  foreign  nations. 

* 

This  technical  report  has  been  renewed  and  is  approved  for  publication. 


FOR  THE  COMMANDER 


THOMAS  J.  MOORE,  Chief 
Biodynamics  and  Biocommunications  Division 
Crew  Systems  Directorate 
Armstrong  Laboratory 


REPORT  DOCUMENTATION  PAGE 


Form  Approved 
0MB  No.  0704-0188 


fnr  rhit  rniiprt.nn  nf  information  is  estimated  to  averaae  1  hour  per  respor^se.  including  the  time  for  reviewing  instructions,  searching  existing  data  source. 
aa?he^mrand  Lintlmi^g  t^  information.  Send^comments  regarding  this  burden  estimate  or  ^ny  other  as^a  of  t^ 

for  reducing  this  burden,  ro  Washington  Headquarters  Services  Direaorate  '-J^ormat^" 

Davis  Highway.  Suite  1204,  Arlington,  VA  22202-4302,  and  to  the  Office  of  Management  and  Budget,  Paperwork  Reduction  Project  (0704-0188),  Washington,  DC  20503. 


1.  AGENCY  USE  ONLY  (Leave  blank)  2.  REPORT  DATE 

DECEMBER  1993 


4.  TITLE  AND  SUBTITLE 

Efficient  Coordination  of  an  Anthropomorphic 
Telemanipulation  System 


^  6.  AUTHOR(S) 

I  Ming  Z.  Huang 


3.  REPORT  TYPE  AND  DATES  COVERED 

INTERIM  REPORT  -  .lIJNE  1993  TO  AUGUST  199 


5.  FUNDING  NUMBERS 

PE  -  65502F 
PR  -  7231 
TA  -  38 
WU  -  08 


f  7,  PERFORMING  ORGANIZATION  NAME(S)  AND  ADDRESSEE S) 

[  Armstrong  Laboratory,  Crew  Systems  Directorate 
'  Bioacoustics  and  Biocommunications  Division 
:  Human  Systems  Center 
I  Air  Force  Materiel  Command 
i  Wright-Patterson  AFB  OH  45433-7901 


9.  SPONSORING /MONITORING  AGENCY  NAME(S)  AND  ADDRESS(ES) 


8.  PERFORMING  ORGANIZATION 
REPORT  NUMBER 


AL/CF-TR-1995-0120 


10.  SPONSORING /MONITORING 
AGENCY  REPORT  NUMBER 


12a.  DISTRIBUTION /AVAILABILITY  STATEMENT 


12b.  DISTRIBUTION  CODE 


Approved  for  public  release;  distribution  is  unlimited. 


13.  ABSTRACT  (Maximum  200  words) 

This  report  documents  the  development  of  coordination  algorithms  for  control 
implementation  of  an  anthropomorphic  telemanipulation  system  presently  at  Wright- 
Patterson  Air  Force  Base.  The  telemanipulation  system,  which  is  to  be  used  as  a 
research  platform  in  facilitating  studies  on  human  sensory  feedback,  comprises  a 
7  degree-of- freedom  (d.o.f.)  force-reflecting  exoskeleton  master  and  a  6  d.o.f. 
articulated  slave  robot.  The  approach  taken  in  the  development  emphasizes  the 
practical  issue  of  computation  efficiency— a  primary  concern  for  satisfactory 
real-time  operations.  The  algorithms  presented  here  have  been  fully  tested  and 
implemented  on  the  system.  Implementation  results  indicate  that  at  least  a 
five-fold  improvement  on  the  control  sampling  rate  has  been  achieved  (from  11  Hz 
to  62  Hz  on  a  68020-based  VME  board).  Other  practical  issues  of  implementation 
are  also  discussed. 


14.  SUBJECT  TERMS 

kinematics,  Jacobian,  bioengineering,  telepresence,  exoskeleton, 
robotics,  man-machine  systems,  force-reflecting  display 


15.  NUMBER  OF  PAGES 

33 


16.  PRICE  CODE 


17.  SECURITY  CLASSIFICATION  I  18.  SECURITY  CLASSIFICATION  I  19.  SECURITY  CLASSIFICATION  20.  LIMITATION  OF  ABSTRACT 
OF  REPORT  OF  THIS  PAGE  OF  ABSTRACT 

UNCLASSIFIED  UNCLASSIFIED  UNCLASSIFIED  UNLIMITED 


NSN  7540-01-280-5500 


Standard  Form  298  (Rev.  2-89) 

Prescribed  by  ANSI  Std.  Z39-18 
298-102 


This  page  intentionally  left  blank. 


THIS  DOCUMENT  IS  BEST 
QUALITY  AVAILABLE.  THE 
COPY  FURNISHED  TO  DTIC 
CONTAINED  A  SIGNIFICANT 
NUMBER  OF  PAGES  WHICH  DO 
NOT  REPRODUCE  LEGIBLY. 


PREFACE 


The  support  of  this  work  as  part  of  Summer  Faculty  Research  Program  sponsored 
by  the  Air  Force  Office  of  Scientific  Research  (AFOSR)  is  gratefully  acknowledged.  The 
author  also  wishes  to  express  his  appreciation  for  help  received  from  Armstrong 
Laboratory,  Bioacoustics  and  Biocommunications  Branch  (AL/CFBA)  at  Wright-Patterson 
Air  Foce  Base,  Ohio.  Special  thanks  go  to  members  of  the  Human  Sensory  Feedback 
group;  Capt.  P.  Whalen,  Capt.  D.  Nelson,  Lt.  C.  Hasser,  Mr.  M.  Crabill  and  Mr.  T. 
Mosher. 


TABLE  OF  CONTENTS 


Page 

INTRODUCTION  . 1 

COORDINATION  OF  THE  FREFLEX  MASTER  ROBOT . 4 

Forward  Position  Kinematics  . 4 

Jacobian  Formulation  . 11 

Gravity  Compensation  . 13 

Procedure  for  Computing  Gravity  Torques . 16 

Kinematic  Coupling  Relationship  . 17 

Joint  Torque  Decomposition . 20 

POSITION  COORDINATION  FOR  THE  SLAVE  MANIPULATOR . 22 

DISCUSSION  AND  CONCLUSION  .  26 

REFERENCES . 27 


iv 


LIST  OF  FIGURES 


Page 

Figure  1 .  The  Force  REFLecting  EXoskeleton  (FREFLEX)  Master  Robot . 2 

Figure  2.  Schematic  of  the  FREFLEX  and  Definitions  of  Kinematic  Frames  with 

Corresponding  Table  of  D-H  Parameters  . 5 

Figure  3 .  Kinematic  Illustration  of  CG  Position  of  j-th  Link  Relative  to  Axis  i 

(Frame(i-l))  . 

Figure  4.  Cable  Drive  System  of  the  FREFLEX . 18 

Figure  5.  Schematic  of  MERLIN  Robot  and  Definition  of  Kinematic  Frames  with  Its 

Corresponding  Table  of  D-H  Parameters  . 23 


V 


This  page  intentionally  left  blank. 


1 


VI 


INTRODUCTION 


Recently,  studies  in  telepresence,  which  involve  man-in-the-loop  control  of 
sensory-rich,  remotely  operated  robotic  systems,  have  emerged  as  a  new  critical  area  of 
research  and  development.  This  is  due  to  the  increasing  recognition  and  acceptance  of 
telerobotic  manipulation  technology  as  being  a  viable  solution  for  remote  operations  in 
unstructured  and/or  hazardous  environments,  such  as  space,  undersea,  or  nuclear  sites  [1- 
3]. 


For  years,  man-machine  interface  research  has  been  a  main  thrust  of  the  efforts  in 
the  Air  Force  to  improve  performance  and  effectiveness  of  its  crews.  Under  the  Crew 
Systems  Directorate,  the  Armstrong  Laboratory  at  Wright-Patterson  AFB  conducts  the 
Human  Sensory  Feedback  (HSF)  research  program,  which  is  charged  with  the  mission  to 
investigate  telepresence  and  its  related  issues.  Among  the  on-going  research  activities  of 
the  HSF  program,  main  emphasis  is  currently  concentrated  on  characterization  of  the  role 
of  human  sensory  feedback  in  the  following  three  key  aspects:  coarse  positioning  (large 
scale  motion  associated  with  the  human  arm  and  wrist),  fine  manipulation  (small  scale 
motion  associated  with  the  human  hand),  and  tactile  feedback. 

A  unique  telemanipulation  test  platform  has  been  designed  and  built  to  support  the 
research  in  the  coarse  manipulative  human  sensory  feedback.  The  platform  is  unique  in 
that  it  uses  a  custom-built,  anthropomorphic  exoskeleton  capable  of  force  reflection  as  the 
master  control  arm  to  command  a  kinematically  dissimilar  slave  (a  revolute-jointed 
industrial  type)  robot.  The  Force-REFLecting  EXoskeleton  (FREFLEX)  master  is  a  seven 
degree-of-freedom,  cable-driven  robot  that  was  designed  specifically  to  provide  mobility 
and  range  of  motion  similar  to  that  of  a  human  arm  (Figure  1).  The  use  of  such  an 
anthropomorphic  exoskeleton  as  the  master  controller  enables  the  operator  to  generate 
motions  and  to  react  to  forces  encountered  during  manipulation  in  a  natural  way,  a  key 
functional  requirement  in  human  sensory  feedback  studies. 

Force  reflection  on  the  FREFLEX  is  achieved  by  its  controller  generating 
appropriate  antagonistic  actions  through  cables  (tendons)  and  pulleys,  driven  by  brushless 
DC  motors.  While  use  of  cables  and  pulleys  makes  it  possible  to  drive  distal  links  from 
actuators  mounted  at  the  base,  significantly  reducing  overall  weight  of  the  arm,  such  an 
actuation  scheme  results  in  severe  cross-coupling  between  motions  of  the  joints  and, 
consequently,  there  is  coupling  between  joint  torque  commands.  Given  a  certain 
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Figure  1 .  The  Force  REFLecting  EXoskeleton  (FREFLEX)  Master  Robot 
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force/moment  to  be  reflected  by  the  FREFLEX,  it  is  necessary  to  identify  the  coupling 
relationship  in  order  to  compute  actual  torque  commands  at  the  actuators. 

When  performing  telemanipulation,  it  is  desirable  that  the  master  controller  appears 
as  being  "weightless"  to  the  operator.  Besides  the  obvious  benefit  of  reducing  operator 
fatigue,  gravity  and  inertia  compensation  increases  the  fidelity  of  manipulative  interaction 
and  hence  the  overall  system  performance.  Update  rate  in  the  control  system  is  another 
factor  which  affects  the  overall  system  performance.  The  system  will  feel  sluggish  or  even 
become  unstable  if  the  cycle  time  is  too  large.  In  general,  it  is  necessary  to  have  an  update 
rate  of  20  to  200  Hz  to  ensure  satisfactory  real-time  performance. 

In  this  report,  we  present  analyses  and  solutions  to  the  problems  of  coordination 
concerning  the  control  implementation  of  the  telemanipulation  system  as  described  above. 
Specifically,  kinematic  models  for  both  the  FREFLEX  and  the  slave  (MERLIN^^  6500  by 
American  Robot  Corp.)  and  their  related  kinematics  solutions  (both  position  and  velocity) 
are  developed.  In  addition,  algorithms  for  the  FREFLEX  to  compensate  gravity  loads  and 
compute  joint  torques,  including  identification  of  the  kinematic  coupling  relationship  for  the 
actuators  and  the  joints,  is  also  presented.  It  is  noted  that  throughout  the  following 
development  of  algorithms  we  have  taken  special  care  to  optimize  computational  efficiency. 
As  a  result,  a  five-fold  improvement  of  the  overall  system  update  rate  has  been  achieved 
with  their  implementation. 
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COORDINATION  OF  THE  FREFLEX  MASTER  ROBOT 


Coordination  for  the  FREFLEX  can  be  divided  into  the  following  stages  of 
computation:  forward  position  kinematics,  Jacobian,  gravity  compensation,  kinematic 
coupling,  and  joint  torque  decomposition.  Note  that  for  a  single  microprocessor  system  all 
of  these  computations  must  be  completed  before  the  next  update  to  .the  controller  can  be 
made;  in  other  words,  the  system  sampling  rate  is  dictated  by  the  overall  efficiency  of  these 
computations.  Consequently,  when  formulating  the  solution  to  each  stage,  a  sensible 
guideline  would  be  to  develop  the  algorithms  in  such  a  way  that  all  items  should  be 
computed  only  once,  and  any  computation  which  occurs  in  the  later  stages  should  take 
maximum  advantage  of  what  has  been  computed  previously. 

Forward  Position  Kinematics 

Forward  position  kinematics  refers  to  the  following  problem:  given  a  set  of 
measured  joint  positions,  compute  the  position  and  orientation  of  the  FREFLEX  hand  grip 
(or  any  conveniently  chosen  point  of  interest)  in  the  Canesian  space.  The  resulting  pose 
(combined  position  and  orientation)  is  then  used  in  the  inverse  kinematics  solution  for  the 
slave  robot  (to  be  described  later)  to  yield  the  corresponding  joint  commands  for  the  slave 
to  be  driven  to  that  same  pose.  Note  that  this  transformation  of  position  command  in 
Cartesian  space  is  necessary  whenever  the  master  and  the  slave  robots  are  of  different 
kinematic  structures. 

To  establish  a  kinematic  model  for  the  FREFLEX,  we  adopt  the  so-called  Denavit- 
Hartenberg  (D-H)  modeling  convention  with  the  frame  assignment  scheme  similar  to  that 
adopted  in  [4]  and  [5].  Figure  2  shows  a  schematic  of  the  FREFLEX  with  definitions  of 
all  the  kinematic  frames  and  a  table  of  their  corresponding  D-H  parameters.  Based  on  the 
above  D-H  convention,  a  4x4  homogeneous  transformation,  denoted  as  ‘‘^T.,  can  be 

derived  to  represent  the  position  and  orientation  of  frame  i  relative  to  the  frame  (i-1);  refer 
to  [4]  or  [5].  It  can  be  easily  shown  that  the  coordinate  transformation  representing 
position  and  orientation  of  the  last  frame  (frame  7  for  the  FREFLEX)  relative  to  the  base 
frame  (frame  0)  can  be  obtained  as: 


“i 

01 

1 

0 

0 

90° 

01 

a 

0 

0 

-120° 

02 

1.969" 

14.640" 

+120° 

03 

H 

-1.969" 

0.625" 

-70° 

04 

0 

11.770" 

+70° 

05 

0 

0 

+90° 

06 

H 

1.68" 

0 

0° 

07 

Figure  2.  Schematic  of  the  FREFLEX  and  Definitions  of  Kinematic  Frames  with 
Corresponding  Table  of  D-H  Parameters 
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or  equivalently,  in  terms  of  ‘  'R.  and  ‘"‘q^  as: 

0t>  _  Oq  lr>  6t> 

rv-^  —  Jtvj  rv2***  -^7 

V  =  °q.  +  «R/q,  +  '>R/q3  +  ...+  «R/q, 

where 

c0i  -sGiCtti  sSjStt;  ajcGi' 

i-ij  ^  sGi  cGiCtti  -cGiStti  ajsG;  ‘’’R;  ‘"‘qi’ 

0  Stt;  Ctti  d;  [0  0  0  1 

_  0  0  0  1  J 


It  is  remarked  that,  while  both  equations  (1)  and  (2)  are  completely  equivalent,  Eq.  (2) 
results  in  better  computation  efficiency  by  separating  the  computations  into  rotational  and 
translational  parts.  By  the  author's  own  experience,  the  two-part  form  of  Eq.  (2)  has  also 
been  found  to  be  more  amenable  to  manipulate  symbolically,  particularly  in  deriving  the 
inverse  kinematics  solution.  For  example,  in  the  FREFLEX,  from  its  parameter  table  we 
have:  °q,  =  =  ^q^  =  0.  This  immediately  leads  to  a  simplification  on  the  translational 

part  of  Eq.  (2)  to:  ‘'q^  =  ''R2^q3  +  ”R3^q4  +  °R4"*q5  +  ^Re^Q?  which  is  clearly  easier  to 
manipulate,  although  the  rotational  part  remains  unchanged.  The  inverse  kinematics 
solution  for  the  slave  robot  (MERLIN),  to  be  included  later,  was  also  reached  based  on  the 
above  procedure. 


Note  that  if,  instead  of  the  last  frame  (frame  7),  the  position  and  orientation  of 
frame  k  is  to  be  computed,  both  of  the  above  equations  still  hold  with  only  a  change  in  the 
upper  index  from  7  to  k  needed.  It  can  be  seen  that  an  efficient  way  to  compute  the  pose  of 
any  link  frame  is  to  do  so  sequentially  by  starting  with  k=l  (from  the  base)  and  then 
progressing  outward.  In  fact,  it  is  also  desirable  in  practice  to  facilitate  the  forward 
kinematics  computations  so  that  the  positions  and  orientations  of  all  the  link  frames  (not 
just  the  hand  grip)  are  easily  accessible  should  they  be  needed  in  subsequent  computations. 
Based  on  Eq.  (2),  an  outward  iteration  algorithm  which  computes  the  pose  of  each  link 
frame  starting  with  the  base  frame  can  be  implemented  using  the  following  recursive 
relationships  (i  =  1  to  k): 


“q.  =  +  %_r\ 

%  =  -  R. 


(3) 
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Knowing  the  position  and  orientation  of  each  link  frame  (relative  to  the  base),  it  is 
now  straightforward  to  obtain  the  absolute  position  of  any  given  point  in  any  link.  Let  ‘p 
be  the  position  vector  of  a  point  in  link  frame  i;  then  its  corresponding  position  in  the  base 
frame,  °P,  is  given  by: 

°p=%'p+\  (4) 


In  the  FREFLEX  implementation,  a  forward  kinematics  solution  has  been 
developed  using  the  above  recursive  scheme.  The  solution  for  the  pose  of  each  link  frame 
is  obtained  in  the  form  of  explicit  analytic  expressions  for  optimal  efficiency.  It  is 
recognized  that  one  may  achieve  reasonable  efficiency  by  direct  numerical  computations 
using  the  same  recursive  scheme  with  a  carefully  coded  algorithm.  However,  to  guarantee 
optimal  efficiency  one  must  ensure  all  necessary  terms  are  computed  once  and  only  once, 
which  requires  being  able  to  identify  and  eliminate  redundant,  repetitious  computations  of 
those  common  terms  which  may  be  embedded  in  more  than  one  expression.  This  can  be 
accomplished  only  by  going  through  the  process  of  analytic  derivation.  It  is  recommended 
that  this  be  done  whenever  efficiency  is  a  critical  factor  in  system  performance. 


The  following  describes  the  details  of  the  derivation  and  the  results  of  the  forward 
kinematics  solution  for  the  FREFLEX.  Referring  to  the  D-H  parameter  table  of  FREFLEX 
(Figure  2),  the  corresponding  link  transformations  between  frame  (i-1)  and  i  are: 
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\=\=\=0 


\  = 

'1.969  003* 
1.969se3 

;  "94  = 

■-1.969C04' 

-1.969S04 

II 

0 

0 

;  '97  = 

‘1.68C07' 

1.68S07 

_  14.640  _ 

.625 
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X 

b  =  sin(-120°);  h  =  cos(-70°);  f  =  sin(70°) 

0,  if  frame  7  is  at  the  base  of  the  handgrip. 
i-Lgrip,if  frame  7  is  at  the  center  of  the  handgrip 


Following  the  algorithm  given  by  Eq.  (3),  with  i  =1  to  7,  the  transformations  in  the  form 
directly  usable  in  Eq.  (4)  are  derived  symbolically  below. 
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.U31 

U32 

U33. 

where  e,, 

^3 

3 

^21  ~  ^ 

^21^ 

■3  +  b 

22  S3 

®31 

^12 

=  b,i 

S3  -* 

3 

^22  ~  ^ 

^21^ 

3-b: 

22^3 

^32 

~  ^31^3  ^32^3 

=  bjjSj—  b32C3 


i  =  4: 


Uj  —  1.969rjj+.625U[3 

’v," 

»q,  =  <’q,  +  “R3’q.= 

U2-  1.969r2,+.625u23 

= 

V2 

U3-  1.969r3,+.625u33 

.''3. 

C4  -hS4  fS4 

S4  hc4  -fcj 

0  f  h 


r„  -hri2  +  fu,3 
h\  -K2  +  fU23 
r3i  -hr3,  +  fu33 


+  hu^3 

"Vll 

V12 

Vl3 

+  hu23 

= 

V21 

V22 

V23 

+  hU33 

.''31 

V32 

V33. 

c: 

U12 

Ui3 

°R4  =  “R3'R4  = 

U2, 

^22 

^^23 

U31 

^32 

^33 

where  r,,  =  u,,C4  +  U12S4 

^2  ~  *^11^4  ~  ^12^4 


^21  ~  ^21^4  ^22^4 

^22  ~  ^21  ^4  ~  b22C4 


^31  ~  ^31^4'^  ^32^4 

r32=U3lS4-U32C4 
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\=\+X\ 


v,  +  11.77v,3 
V2  +  II.77V23 
_V3  +  11.77V33 


"R,  =  ”R/R,= 


'11^5  ^  ''12'=>5 


r 

11 

V,2 

V13’ 

'cs 

-hsj 

'21 

V22 

'^23 

^5 

hCj 

'31 

V32 

^33. 

0 

-f 

5 

ht„- 

fv,3 

ftll 

+  hv 

V21C5  +  V22S5  ht„-fv„  ft„  +  hv,J  = 


+  ht„-fv„  fu,  +  hv, 


w,,  w 


W21  W22 


where  t„  =  v,2C5-v„S5  t2,  =  V22C5- V2,S5 


12  ^13 
00  no 


W31  W32 


^31  ''32^5 


°q6  =  °q5=  W2 


’w„ 

W,2 

W,3‘ 

’^6 

W2, 

W22 

W23 

^6 

.W3, 

W32 

W33. 

0 

''12^6 

W,3 

WuS 

6-W 

''22^6 

W23 

W2,S, 

6-W: 

''32  ^6 

W33 

W3,S, 

5-W; 

y3i  y32  y33 


°q7-°q5  +  ''*^6^q7=  W2+1.68Z2i  +  Xy23 


w,  +  1.68z,]  +  xy,3 

W2+  I.68Z21  +  xy23 
_W3+  I.68Z31  +  xy33 


yii  yi2  yi3  c,  -Sj  O  Zj,  -yiiS7  +  y,2C7  y,3 

”R7=X'^R7=  y21  y22  y23  S7  C7  0  =  Z21  -y2iS7  +  y22C7 

_y31  y32  y33_.0  0  231  ~y3lS7  +  y32C7  y33^ 

where  Zu  =  ynC7  +  y,2S7  Z2,  =  y2iC7  +  y22S7  Z3,  =  y3jC7  + y32S7 

It  is  noted  again  that  all  terms  which  appear  more  than  once  have  been  identified, 
and  upon  computations,  their  values  are  stored  as  new  parameters  to  avoid  redundant 
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operations.  As  alluded  to  earlier,  this  is  only  possible  when  the  forward  kinematics 
transformations  are  derived  analytically.  We  have  evaluated  the  computational  efficiency  of 
our  algorithm  in  terms  of  the  required  operation  counts  for  addition/subtraction  (A), 
multiplication/division  (M),  and  trigonometric  function  calls  (F).  For  the  FREFLEX,  the 
forward  kinematics  algorithm  calls  for  a  total  of  (64A+133M+14F),  as  compared  to  the 
(162A+216M+14F)  needed  if  implemented  with  direct  numerical  computation  using  Eq. 
(3). 


Jacobian  Formulation 


As  is  well  known  in  robotics,  the  Jacobian  is  a  transformation  matrix  which  relates 
differential  motions  (linear  and  angular)  of  the  robot  end-effector  in  the  Cartesian  space  to 
the  corresponding  differential  displacements  at  the  joints.  It  is  also  known  that  in  statics 
consideration  the  same  transfomiation  can  be  used  to  relate  the  external  force  and  moment 
applying  at  the  end-effector  to  the  torques  (or  forces,  if  prismatic)  at  the  joints. 
Mathematically,  the  above  statements  can  be  expressed  using  the  following  equations: 


where 

J :  the  manipulator  Jacobian 

w,  and  v:  the  end-effector  anguUir  and  linear  velocities,  respectively 
M  and  F:  the  resultant  moment  and  force  by  the  end-effector,  respectively 
0 :  nxl  vector  of  joint  rates  (n  =  manipulator's  degree  of  freedom) 
x:  nxl  vector  of  joint  torques/forces 
and  superscript  T  indicates  the  matrix  transpose  operation. 


Although  the  above  relationships  may  have  been  seen  in  many  texts,  it  would  be 
helpful  to  clarify  a  few  points  when  considering  their  applications.  First  and  foremost,  care 
must  be  taken  to  ensure  that  all  vector  quantities  involved  be  formulated  with  respect  to  the 
same  coordinate  frame  of  reference.  In  addition,  point-specific  vectors,  such  as  linear 
velocity  v  and  moment  M,  must  be  given  such  that  they  are  all  relative  to  the  same  point  of 
reference  which  was  used  to  formulate  the  Jacobian.  In  other  words,  the  Jacobian  may 
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take  on  various  different  forms,  depending  upon  the  reference  frame  and  the  reference  point 
chosen  (to  describe  v  or  M)  during  its  formulation. 


To  emphasize  the  distinction  between  the  various  available  forms,  we  shall  denote 
the  Jacobian  with  ,  in  which  the  leading  superscript,  m,  specifies  the  frame  and  the 
trailing  subscript,  k,  gives  the  reference  point.  In  general,  ”  for  a  robot  of  n  degrees  of 
freedom  can  be  formulated  using  the  following: 


J.= 


Ui 

PiXUi 


i  =  l,2. 


n 


(7) 


in  which  U;  is  the  unit  directional  vector  of  joint  axis  i,  and  P;  is  the  position  vector  of  axis 
i  with  respect  to  the  reference  point  k;  of  course,  both  vectors  are  expressed  in  frame  m. 
Note  that  the  above  form  of  column  vector  in  Eq.  (7)  only  applies  to  robots  with  all 
revolute  joints.  In  the  case  for  robots  with  prismatic  joints,  say  joint  j,  then  the  j-th  column 
should  be  replaced  with  [(0,0,0);  Uj' 


For  the  FREFLEX,  we  have  chosen  to  use  “jg,  which  is  to  formulate  the  Jacobian 

in  the  base  frame  (frame  0)  with  its  origin  as  the  reference  point.  Again,  the  main 
consideration  here  is  to  minimize  computation  cost.  The  reason  to  use  “jg  is  two-fold: 

first,  the  origin  of  frame  0  is  the  point  of  concurrency  of  axes  1,  2,  and  3;  and  second,  all 
the  terms  required  to  formulate  the  Jacobian  are  readily  available  from  the  forward 
kinematics  procedure  with  no  further  manipulation  necessary.  As  a  result,  we  arrived  at  the 
following  6x7  matrix  as  the  Jacobian  for  the  FREFLEX: 


0 


0 


Z3 

0 

Qs  X  Z3 


Z4  Z5  Zg 

'’q4  X  Z4  "qj  X  Zj  X  z^ 


(8) 


where,  referring  to  Figure  2,  =  U;  (i=  ]  to  7)  which  is  the  third  column  of  “^R.,  and  °q. 

is,  as  defined  before,  the  position  of  the  origin  of  frame  i  on  axis  (i+1);  all  of  which  can  be 
obtained  directly  from  the  results  of  the  forward  kinematics  computations.  As  can  be  seen 
from  Eq.  (8),  only  four  additional  vector  cross  product  operations  (24M+12A)  are  required 
to  obtain  the  Jacobian. 


The  Jacobian  formulated  here  will  be  used  in  Eq.  (6)  to  compute  the  necessary  joint 
torques  in  order  to  "reflect"  a  certain  external  load  (force  and  moment).  Typically,  the  point 
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at  which  the  load  is  to  be  reflected  will  not  be  the  same  as  the  reference  point  used  by  the 
Jacobian.  Therefore,  before  Eq.  (6)  can  be  applied  it  is  necessary  to  transform  the 
generalized  load  vector  (both  force  and  moment)  to  the  same  reference  point,  which  is  at  the 
base  frame  origin.  For  discussion  sake,  let  the  load  reflection  point  be  at  the  wrist  center  of 
the  FREFLEX.  Typically  this  will  be  the  case  since  the  external  load  will  usually  be 
measured  using  a  force/torque  sensor  mounted  at  the  wrist  of  the  slave  robot.  Then  the 
following  transformations  are  required: 


X  =  “Rs  X 

X  =  “Rs'm„  +  “p„x«Fo 


(9) 

(10) 


where  and  denote  the  measured  force  and  moment  in  the  sensor  frame  (s),  is 
the  coordinate  transfomiation  matrix  from  the  sensor  to  the  base  frames,  and  is  the 
position  of  the  load  reflection  point,  namely  wrist  center,  relative  to  base  frame.  One  can 
easily  generalize  the  above  relationships  to  cases  where  the  point  of  reflection  is  different, 
simply  by  using  the  corresponding  position  vector  of  the  new  load  reflection  point  and,  if 
necessary,  the  appropriate  coordinate  transformation  matrix. 


Gravity  Compensation 


To  facilitate  better  utility  and  reduce  operator  fatigue,  the  FREFLEX  needs  to 
support  its  own  weight  as  it  is  being  moved  about  providing  position  commands  to  control 
the  slave  manipulator.  In  general,  the  capability  of  gravity  compensation  is  necessary  for 
any  master  robot  used  in  telerobotic  systems.  During  each  sampling  period,  this  requires 
computing  first  the  gravity  loads  at  all  joints  and  subsequently  the  torques  needed  from  the 
motors  to  statically  counteract  those  gravity  loads.  In  this  section  we  only  address  the 
former,  which  is  the  computation  of  the  gravity  loads  (torques)  at  the  exoskeleton  joints. 
The  discussion  of  the  latter,  namely  the  decomposition  of  these  computed  joint  torques  into 
the  actual  motor  torques,  is  deferred  to  the  following  sections. 


Consider  the  schematic  of  link  connection  at  a  revolute  joint  in  a  serial  chain  as 
illustrated  in  Figure  3.  The  following  observations  can  be  made:  As  a  result  of  the  serial 
chain  configuration,  the  gravity  load  seen  by  that  joint  will  be  due  to  the  sum  of  moments 
generated  by  weights  of  all  the  links  outboard  from  it  (farther  from  the  base).  And  the  load 
will  vary  as  relative  positions  of  links  change  from  one  configuration  to  another;  in  other 
words,  it  is  position  dependent. 
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axis  1 


Figure  3.  Kinematic  Illustration  of  CG  Position  of  j-th  Link  Relative  to  Axis  I  (Frame  (i-1)) 
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Let  hg  j  represent  the  position  vector  of  the  center  of  gravity  (CG)  of  link  j  relative 
to  joint  axis  i  (Figure  3)  and  the  link  mass  be  given  as  m^.  Then,  with  the  origin  of  frame 
(i-1)  as  the  point  of  reference,  we  can  write  the  moment  at  joint  i  due  to  the  weight  of  link  j 
as: 


Hij  =  K.i  X 


(11) 


Thus,  the  total  moment  due  to  all  outboard  links  (j  ^  i)  is: 

7  7 

j=i  j=i 


And  the  corresponding  joint  torque  (tj)  is  just  the  component  of  total  moment  along  the  joint 
axis: 


X:  =n-  Z: 


i-1 


(13) 


It  can  be  easily  seen  that  computing  j  is  the  key  step  in  the  gravity  torque  computation 
procedure.  The  way  in  which  they  are  computed  directly  affects  the  efficiency  of  this 
algorithm.  A  logical  consideration  here  is  to  take  advantage  of  results  available  from  the 
previous  forward  kinematics  procedure.  This  suggests  that  the  computations  should  be 
made  in  the  base  frame.  We  compute  ^  using  the  following  relationship,  which  can  be 

easily  arrived  from  Figure  3: 


(14) 


where  j  is  the  given  position  of  CG  for  link  j  with  respect  to  its  local  frame  j,  and  “q. 
and  °R.  are,  as  defined  before,  position  and  orientation  matrices  of  frame  j,  both  of  which 

are  readily  available  from  previous  results. 


To  pursue  the  issue  of  efficiency  a  little  further,  computing  in  the  base  frame 
coordinate  has  associated  with  it  another  added  benefit.  For  the  cross  product  calculation  in 
Eq.  (1 1)  it  is  only  necessary  to  compute  two  out  of  the  three  components.  This  is  because, 
when  expressed  in  the  base  frame,  the  gravity  vector  will  typically  have  a  nonzero 
component  along  only  one  coordinate  axis.  For  example,  in  terms  of  the  FREFLEX  base 
frame,  g  =  [-g,  0,  0]  ,  and  thus  the  cross  product  which  results  has  only  nonzero  y  and  z 
components.  As  a  result,  Eqs.  (12)  and  (13)  for  the  FREFLEX  can  be  evaluated  using  the 
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simplified  component  forms  (note  that,  in  what  follows,  the  superscript  0  of  the  h  terms 
given  by  Eq.  (14)  above  has  been  dropped  for  convenience): 
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n.  =  <  n 


"i,y=  -gX(h>j) 

j=l 

«.,z= 


'ti  =  +  ni.,U.  , 


Implementation  of  the  above  equations,  Eqs.  (14)  to  (16),  has  resulted  in  a  highly  efficient 
gravity  torque  computation  algorithm  for  the  FREFLEX  which  requires  only 
(126M+133A+OF)  in  its  calculations.  In  the  following,  we  summarize  the  computation 
procedure  of  the  gravity  torque  algorithm. 

Procedure  for  Computing  Gravity  Torques 


Step  1:  Transform  position  of  j-th  link  CG  with  respect  to  the  i-th  frame  (j  >  i)  in  the 

base  frame  0: 

=  j  =  l,2,...,7 

Step  2:  Compute  position  of  j-th  link  CG  with  respect  to  the  i-th  frame  (j  >  i,  where  i  = 

1  to  7;  and  j  =  i  to  7): 


e.g.,  let  i=l,  j=7: 


X.'lV-'qJ+v, 

Step  3:  Compute  moment  about  the  reference  point  on  axis  i;  i.e.,  the  origin  of  frame  (i- 

1),  due  to  the  weights  of  all  j-th  links: 
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n- 


=  -gZK.ni,) 

j=l 


Step  4: 


The  gravity  torque  for  joint  i  is  the  component  of  the  moment  in  the  direction  of 
its  axis: 


Based  on  the  above  computed  results,  ideally  one  should  be  able  to  completely 
compensate  the  weight  of  all  the  links.  This  of  course  assumes  a-priori  that  the  data  on  the 
inertia  properties  (link  masses  and  CG  locations)  of  the  system  are  correct,  which 
unfortunately  is  typically  not  the  case.  A  practical  method  to  deal  with  this  problem  is  to 
experimentally  "scale"  the  values  of  computed  gravity  torques.  Such  constant  scaling 
factors  can  be  found  one  joint  at  a  time  by  adjusting  from  the  last  one  inward  until  the 
link(s)  supported  by  that  joint  in  effect  begin  to  "float". 

Kinematic  Coupling  Relationship 

As  mentioned  in  the  introduction,  the  cable  and  pulley  actuation  scheme  gives  rise 
to  a  complication  of  cross-coupling  between  motions  at  the  joints  on  the  FREFLEX  and  its 
actuators  at  the  base.  Figure  4  illustrates  the  cable  system  arrangement  used  by  the 
FREFLEX  [6].  In  the  present  scheme,  cables  to  drive  a  distal  joint  are  routed  via  pulleys 
(idlers)  through  all  inboard  joints  which  are  more  proximal  to  the  base.  As  a  consequence, 
when  the  motor  for  the  distal  joint  (the  joint  at  which  the  cable  terminates)  is  actuated, 
torque  is  transmitted  not  only  to  that  distal  joint,  but  to  each  of  the  inboard  joints  of  the 
arm,  which  in  turn  causes  motions  at  all  those  proximal  joints.  This  is  in  contrast  to  the 
conventional  actuation  scheme  in  which  the  motion  of  a  joint  is  dependent  only  on  a  single 
actuator  driving  that  joint. 

To  control  the  FREFLEX,  it  is  necessary  to  identify  the  above  coupling  relationship 
in  order  to  "decouple"  desired  joint  motions  or  torques  into  appropriate  actuator  commands. 
It  should  be  noted  that  the  coupling  as  described  here  is  kinematic  in  nature  in  that  it  arises 

solely  from  kinematic  constraints  due  to  cable  routing,  and  should  not  to  be  confused  with 
the  inertia  coupling  as  seen  in  dynamics.  Let  and  represent,  respectively, 

the  7x1  vectors  of  displacements  at  the  motors  and  their  corresponding  displacements  at  the 
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SHOULDER 


Figure  4.  Cable  Drive  System  of  the  FREFLEX 
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FREFLEX  joints.  Then  they  can  be  related  by  a  (constant  global)  coupling  matrix  of  7x7, 
A,  as  follows: 

=  (17) 

The  above  coupling  matrix  is  constant  throughout  the  FREFLEX  workspace,  since  it  is 
only  a  function  of  geometric  attributes  such  as  relative  locations  of  the  joints  and  pulleys, 
numbers  of  pulleys  used,  and  radii  of  pulleys  [7].  Ideally,  such  a  matrix  can  be  expected  to 
be  lower  triangular. 

In  theory  one  can  attempt  a  direct  approach  to  derive  the  coupling  matrix  based  on 
the  aforementioned  geometric  attributes.  However,  practical  considerations— such  as  errors 
induced  by  uncertainties  as  well  as  inaccuracies  associated  with  geometric  data,  suggested 
that  in  fact  it  would  be  more  reliable  to  identify  the  coupling  relationship  indirectly,  using 
actual  data  of  joint  and  motor  displacements  measured  experimentally. 

Referring  to  Eq.  (17),  it  can  be  easily  seen  that  if  sufficient  measurements  of 
motor  and  A0  are  available  (in  this  case  the  minimum  number  is  7),  then  identification 

of  the  coupling  matrix  A  amounts  to  solving  a  system  of  linear  equations  with  its  elements 
as  the  independent  variables.  Furthermore,  one  can  minimize  the  effect  of  measurement 
noises  by  using  more  measurement  data  than  the  minimum  and  computing  the  generalized 
inverse  solution  of  A.  In  fact,  such  a  solution,  to  be  given  below,  represents  physically  the 
optimum  or  best-fit  set  of  elements  for  matrix  A  in  a  least-squares  sense. 

Let  the  number  of  measurements  be,  say,  n  (n  >  7).  By  using  the  n  measured  joint 
displacement  vectors  as  columns,  a  7xn  joint  measurement  matrix,  defined  as  C,  can  be 
formed.  Similarly,  let  B  represent  the  corresponding  7xn  actuator  measurement  matrix 
comprising  the  correspondingly  ordered  actuator  displacement  vectors  as  columns.  Then 
we  can  obtain  the  least-squares  solution  of  A  by: 

A  =  CBqBB'')‘‘  (18) 

To  acquire  the  necessary  joint  and  actuator  displacement  data,  the  following  process  was 
carried  out  on  the  FREFLEX.  The  FREFLEX  was  moved  to  take  on  different 
configurations  within  its  workspace.  In  the  meantime,  actual  position  readings  from 
potentiometers  at  the  joints  and  resolvers  at  the  actuators  were  recorded  at  a  constant 
sampling  interval.  Having  acquired  the  joint  and  motor  position  readings,  the  difference 
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between  each  consecutive  set  of  data  was  computed  to  give  the  corresponding  displacement 
vectors  which  can  then  be  used  in  Eq.  (18)  to  solve  for  the  coupling  matrix.  It  should  be 
noted  that  during  these  data  collection  movements,  it  is  important  that  care  should  be  taken 
not  to  run  any  joint  up  to  its  mechanical  limits  in  order  to  avoid  artificial  bias  of  joint  data. 


The  following  gives  the  result  of  the  coupling  matrix  we  have  identified  for  the 
FREFLEX  from  a  total  of  16  position  measurements,  namely,  15  sets  of  displacement 
vectors: 


A  = 


-.9617 

.0548 

-.0169 

.0194 

.0065 

-.0024 

-.0255 

.9632 

.9711 

.0039 

.0001 

.0045 

-.0029 

-.0046 

-.0246 

-.4294 

.9672 

.0257 

-.0231 

.0177 

-.0305 

.0292 

.1976 

.7246 

-.7250 

-.0278 

.0310 

-.0408 

.0075 

.4336 

.5890 

.3732 

-1.1949 

-.0052 

-.0032 

-.0052 

.3139 

-.2185 

.2192 

1.2706 

-1.5005 

-.0062 

.0539 

.4949 

-.3120 

.3114 

.0092 

1.4494 

-1.4256 

As  can  be  seen  from  the  result  obtained  here,  the  coupling  matrix  is  in  general  agreement 
with  that  expected  from  theory,  that  is,  elements  in  its  upper  half  of  the  matrix  above  the 
diagonal,  while  not  ideally  all  zeros,  all  have  relatively  small  magnitudes  compared  to  those 
in  the  lower  half.  Furthermore,  it  can  be  seen  that  strong  cross  coupling  exists  between 
joints  5,  6,  and  7  (as  can  be  evidenced  from  unusually  large  values  of  (6,5)  and  (7,6) 
elements),  a  characteristic  which  has  been  regularly  observed  from  actual  behavior  of  the 
FREFLEX. 


Joint  Torque  Decomposition 

The  same  kinematic  coupling  matrix  developed  in  the  previous  section  can  also  be 
used  to  decouple  the  torques  required  at  the  FREFLEX  joints  into  those  needed  to  be 
applied  at  the  motors.  By  principle  of  virtual  work,  a  corresponding  static  coupling 
relationship  can  be  reached  in  a  straightforward  way.  This  is  illustrated  as  follows.  Based 
on  the  input-output  relation  of  (virtual)  work,  we  must  have 


motor  ^  motor  ^  jo int  ’  joint 

And  through  the  use  of  Eq.  (17),  one  can  easily  arrive  at; 

T  —  T 

''motor  ^  ^  joint 


(19) 


(20) 
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With  Eq.  (20),  it  is  now  possible  to  compute  the  torque  commands  which  are 
necessary  to  be  applied  at  the  motors  so  as  to  (1)  counteract  the  gravity  loads  due  to  their 
own  weight,  and  (2)  generate  the  external  load  (force  and/or  moment)  to  be  reflected  to  the 
operator.  Designating  such  motor  torques  in  vector  form  as  we  can  combine 

Eqs.  (6),  (16),  and  (20)  to  yield: 


T  =  A' 

^  motor,  Lola  1  ^ 


“T  •+ J 

^gravity 


M, 


For  clarity,  the  subscript  "gravity"  has  been  added  to  the  first  term  in  the  parentheses  to 
delineate  the  joint  torque  vector  due  only  to  gravity,  which  is  available  from  Eq.  (16) 
directly.  Notice  that  this  result  is  then  negated  to  give  the  joint  torques  needed  to  equilibrate 
the  gravity  loads  before  its  decomposition  into  corresponding  motor  commands.  Similarly, 
the  subscript  "ext"  in  the  second  term  represents  resultant  load  of  force  and  moment  (as 
given  by  Eqs.  (9)  and  (10))  to  be  generated  by  the  motors,  in  addition  to  the  gravity  loads. 
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POSITION  COORDINATION  FOR  THE  SLAVE  MANIPULATOR 


A  MERLIN  6500  robot  (by  American  Robotics  Corp.)  is  used  as  the  slave 
manipulator  which  is  controlled  by  the  operator  through  the  FREFLEX  to  interact  with  the 
environment.  The  robot  is  a  six-degree-of-freedom,  revolute-jointed  industrial  robot  with 
stepper  motor  drives  and  a  geometry  similar  to  that  of  the  commonly  known  PUMA 
manipulator.  Typically  the  MERLIN  can  be  programmed  to  operate  using  its  own  specific 
programming  system,  AR-SMART  [8],  provided  by  the  manufacturer.  In  order  to  achieve 
the  necessary  speed  of  response,  the  programming  system  is  bypassed  and  a  direct 
communication  link  is  implemented  between  the  FREFLEX  and  the  MERLIN  controllers 
through  a  high  speed  interface  protocol  [8]. 

During  system  operations,  Cartesian  position  and  orientation  ("pose")  data  of  the 
FREFLEX  master  is  sent  to  the  MERLIN  controller  which,  in  turn,  must  drive  the  slave 
robot  to  the  same  input  position  and  orientation  (which,  of  course,  is  now  relative  to  the 
slave  base  frame).  Obviously,  for  the  MERLIN  controller,  it  is  necessary  to  compute  the 
corresponding  joint  angles  for  a  given  pose  command.  Again,  since  these  joint  angles  must 
be  updated  at  every  sampling  time  as  new  commands  to  joint  servos,  the  computations 
must  be  made  as  efficient  as  possible. 

In  the  following,  we  present  for  completeness  the  results  from  the  inverse 
kinematics  analysis  developed  for  the  MERLIN  to  facilitate  efficient  computations.  While 
details  of  analysis  in  arriving  at  the  solutions  will  not  be  reported  here,  we  note  that  the 
approach  taken  follows  the  one  referenced  in  earlier  discussion,  in  which  the  kinematic 
equation  is  separated  into  translational  and  rotational  parts.  For  MERLIN,  we  have 
obtained  in  explicit  closed  form  the  resulting  set  of  joint  angle  solutions.  Based  on  the 
frames  and  D-H  parameters  defined  in  Figure  5,  the  complete  solutions  are  given  as 
follows. 

Let  the  desired  position  tran.sformation  be  given  by: 
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Figure  5.  Schematic  of  MERLIN  Robot  and  Definition  of  Kinematic  Frames 
with  Its  Corresponding  Table  of  D-H  Parameters 
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And  define: 


Next  form; 


Then: 


65  3.tsn2  (  bj3C4  ^>2384 ,  b33) 

~  &tan2  ((b2iC4  —  bjjS4) ,  (b22C4  —  b[2S4)^ 


It  is  important  to  note  that  the  computations  should  be  carried  out  in  the  same  order  as 
presented  above. 
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It  might  be  helpful  to  make  a  few  general  comments  concerning  the  practical 
implementation  of  the  inverse  kinematics  solution.  Note  that,  although  in  theory  multiple 
solutions  are  possible  to  achieve  a  given  end-effector  position  (e.g.,  a  total  of  eight 
solutions  for  the  MERLIN  geometry),  restrictions  from  physical  and/or  operational 
constraints  will  often  render  fewer  feasible  solutions  in  reality.  As  a  case  in  point,  the 
MERLIN  used  in  this  study  was  physically  constrained  to  maintain  a  left-shoulder 
configuration,  stipulating  CTj  being  always  positive.  If,  in  addition,  an  elbow-up  constraint 
(O2  negative)  is  also  imposed,  the  number  of  feasible  solutions  then  reduces  to  two,  given 
by  the  remaining  two  values  of  S3  corresponding  to  different  configurations  of  the  wrist 
assembly.  Which  of  the  two  to  pick  can  depend  on  operational  consideration,  such  as 
selecting  the  one  closest  to  the  current  positions,  namely  the  one  which  results  in  the 
minimum  joint  motions. 

Another  seemingly  trivial  but  nevertheless  useful  note  concerns  the  way  in  which 
the  joint  angle  command  is  expressed.  As  a  rule  of  good  practice,  one  should  express  the 
angles  returned  from  trigonometric  operation  (e.g.,  atan2)  in  a  sign  consistent  manner,  i.e., 
between  +/-  180  degrees,  particularly  when  they  are  to  be  used  as  joint  commands.  This 
arises  from  the  fact  that  most  joint  servos  are  direction-sensitive  to  the  signs  of  angles; 
angular  joint  commands  of,  say,  150  degrees  and  -330  degrees  are  likely  to  cause  the  motor 
to  turn  in  opposite  directions,  although  they  both  will  reach  the  same  angular  position 
eventually. 
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DISCUSSION  AND  CONCLUSION 


We  have  presented  the  development  of  coordination  algorithms  for  control 
implementation  of  a  telemanipulation  system  consisting  of  an  exoskeleton  master  and  an 
articulated  slave  manipulator.  Physical  modeling  to  analyze  mechanics  of  each  subsystem 
was  established,  and  algorithms  for  its  coordination  were  subsequently  formulated. 
Throughout  the  development,  special  emphasis  was  placed  on  computation  efficiency  due 
to  its  role  in  system  performance. 

The  algorithms  presented  in  this  report  have  been  fully  tested  and  implemented  on 
the  robot  subsystems  and,  as  a  result,  significant  improvements  in  the  overall  system 
performances  have  been  achieved.  In  an  initial  implementation  on  the  FREFLEX,  using 
the  same  processor  (68020),  a  greater  than  five-fold  speed  increase  in  computation  cycle 
time  (from  90  down  to  16  msec)  was  achieved  using  the  present  algorithm  as  compared  to 
an  earlier  one.  One  can  further  improve  the  sampling  rate  by  (1)  using  a  faster  processor, 
and  (2)  updating  the  gravity  torque  commands  (the  most  computationally  intensive  part) 
less  frequently.  The  latter  is  justified  in  light  of  slowly  changing  configurations  of  the 
FREFLEX  when  being  controlled  by  the  operator.  Both  of  the  above  steps  have  been  done 
and,  as  of  this  writing,  a  FREFLEX  motor  torque  update  rate  of  287  Hz  has  been  attained 
using  a  newly  replaced  68030  processor  and  updating  gravity  torque  commands  once  every 
10  cycles.  The  MERLIN  inverse  kinematics  solution  is  computed  on  a  68030  processor  in 
less  than  the  minimum  4  ms  MERLIN  controller  update  period. 
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